#include <mc_ros/processor/processor_p2p.h>


// 构造函数
ProcessorP2P::ProcessorP2P(boost::shared_ptr<PlannerBase> path_planner_ptr,
            boost::shared_ptr<ControllerBase> localControllerPtr,
            boost::shared_ptr<MCVisualization> mcVisualizationPtr):
    globalPathPlannerPtr_(path_planner_ptr), localControllerPtr_(localControllerPtr),mcVisualizationPtr_(mcVisualizationPtr){
    __rotate_start_direction = false;
    smResetIdle();
}

// 析构函数
ProcessorP2P::~ProcessorP2P(){

}

// 设置目标点数据
void ProcessorP2P::setGoal(geometry_msgs::PoseStamped pose_stamp){
    LOG(INFO) << "[ProcessorP2P] set goal pose!";
    goalPoseStamp_ = pose_stamp;
    setGoalPose_ = true;
}

// 空闲状态
void ProcessorP2P::smResetIdle(){
    pathGlobal_.clear();
}

// 执行中预处理函数
void ProcessorP2P::smExecutingEntry(){
    if(!setGoalPose_){
        LOG(WARNING) << "please set goal pose!";
        setEvent(StateMachine::Event::EV_CANCEL);
        return;
    }
    if(!globalPathPlannerPtr_){
        LOG(WARNING) << "planner_ptr is nullptr!";
        setEvent(StateMachine::Event::EV_CANCEL);
        return;
    }

    // 生成路径
    vector<geometry_msgs::Pose> poses;
    mc::status gen_state = globalPathPlannerPtr_->genPath(
        trajectoryROS_.toROSPose(robotPose_), goalPoseStamp_.pose, poses);
    __rotate_start_direction = false;
    if(!gen_state.success || poses.empty()){
        LOG(WARNING) << "generate path failed!";
        setEvent(StateMachine::Event::EV_CANCEL);
        return;    
    }
    pathGlobal_.resize(poses.size());
    for(size_t i = 0; i < poses.size(); ++i){
        pathGlobal_[i] = msg_convert::toPoseStamped(poses[i], "map");
    }
    localControllerPtr_->updateGlobalPath(pathGlobal_);
    mcVisualizationPtr_->publishGolbalPath(pathGlobal_);
    cout << " global_path front: " << msg_convert::toString(pathGlobal_.front().pose) << 
        ", back: " << msg_convert::toString(pathGlobal_.back().pose) << endl;

}

// 执行中处理函数
void ProcessorP2P::smExecutingRunning(){
    try{
        geometry_msgs::Twist cmd_vel;
        geometry_msgs::Twist curr_vel = baseOdometry_.twist.twist;

        if(pathGlobal_.empty()){
            throw std::runtime_error("pathGlobal_ is empty!");
        }      

        // 裁剪路径
        localControllerPtr_->pruningPath(1.0f);

        // 碰撞检测
        int collision_index = -1;
        if(localControllerPtr_->isPathCollision(collision_index)){
            LOG(WARNING) << "path collision!";

            // 重规划路径
            if(!localControllerPtr_->replanPath(collision_index)){
                throw std::runtime_error("replan path failed!");
            }
        }

        // 发布全局路径
        vector<geometry_msgs::PoseStamped> path_poses
            = localControllerPtr_->getGlobalPath();
        mcVisualizationPtr_->publishGolbalPath(path_poses);
        
        // 获取前瞻点
        mc::TrajectoryPoint2D lookahead_goal;
        int lookahead_goal_index = -1;
        double lookahead_distance = localControllerPtr_->getLookAheadDistance(0.2);
        if(!localControllerPtr_->getLookaheadGoal(lookahead_distance, lookahead_goal, lookahead_goal_index)){
            throw std::runtime_error("can not get lookahead goal!");
        }

        auto lookahead_posestamp = trajectoryROS_.toROSPoseStamped(lookahead_goal);
        LOG(INFO) << "[smExecutingRunning] lookahead_goal: " << msg_convert::toString(lookahead_posestamp.pose)
            << ", distance: " << lookahead_distance 
            << ", index: " << lookahead_goal_index;
        mcVisualizationPtr_->publishLookaheadPose(lookahead_posestamp, "map");            


        // 判断是否到达终点
        if(localControllerPtr_->isGoalReached(robotPose_, lookahead_goal)){
            ROS_INFO("The robot reaches the goal point.");
            localControllerPtr_->sendEmptyTwist();
            setEvent(StateMachine::Event::EV_OVER);
            return;
        }   

        // 起点角度旋转, 减少不必要转弯次数
        if(!__rotate_start_direction){
            LOG(INFO) << "__rotate_start_direction: " << __rotate_start_direction;
            if(!localControllerPtr_->isAngleReached(robotPose_, lookahead_goal)){
                localControllerPtr_->rotateToTGoal(robotPose_, lookahead_goal, cmd_vel);
                LOG(INFO) << "[smExecutingRunning | rotateToTGoal] cmd_vel: " << msg_convert::toString(cmd_vel);
                localControllerPtr_->sendVelocity(cmd_vel);
            }
            else{
                __rotate_start_direction = true;
                localControllerPtr_->sendEmptyTwist();
            }
            return;
        }   


        // 执行局部路径
        mc::status status = localControllerPtr_->computeTwist(lookahead_goal, curr_vel, cmd_vel);
        LOG(INFO) << "[smExecutingRunning | computeTwist] cmd_vel: " << msg_convert::toString(cmd_vel);
        if(status.success){
            localControllerPtr_->sendVelocity(cmd_vel);
        }else{
            setEvent(StateMachine::Event::EV_OVER);
        }

    }
    catch (const std::runtime_error& e) {
        std::cerr << "[ProcessorP2P | smExecutingRunning] error: " << e.what() << std::endl;
        localControllerPtr_->sendEmptyTwist();
    }

}

void ProcessorP2P::smExecutingExit(){
    // setGoalPose_ = false;
    // localControllerPtr_->PublishEmptyTwist();
}


// 暂停中处理函数
void ProcessorP2P::smPauseRunning(){
    localControllerPtr_->sendEmptyTwist();
}

// 取消中处理函数
void ProcessorP2P::smCancelRunning(){
    localControllerPtr_->sendEmptyTwist();

}

// 结束中处理函数
void ProcessorP2P::smFinishedRunning(){
    localControllerPtr_->sendEmptyTwist();

}